#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include "urobot.h"
#include "target.h"
#include "services.h"
#include "sms.h"

//#include "utils.h"
#include "turtlebot.h"
#include "roomba.h"

#define USBSERIAL	"/dev/ttyUSB0"

#define ADCID		0x00AD
#define ACCID		0x00AC
#define CAMID		0x0C

pthread_t sensor_thread_id;
void *sensor_thread(void *arg);

/* Car Status Check */
int go = 0;
int back = 0;
int left = 0;
int right = 0;
int fog = 0;
int front = 0;
int emer = 0;
int dm_right = 0;
int dm_left = 0;
int breakl = 0;

mode roomba_mode = MODE_UNKNOWN;
int roomba_fd;

int camera_id = 0;
int camera_type = 0;
char camera_encoding[] = "multipart-jpeg";

/* globals */
static globals global;
struct termios oldtio;

int init_camera(void);
int init_sensor(void);

int open_serial(void)
{
        struct termios newtio;
        int fd;

        fd = open(USBSERIAL, O_RDWR | O_NOCTTY );
        if (fd < 0){
                perror("/dev/ttyUSB0 is not Exist!");
                exit(EXIT_FAILURE);
        }
        tcgetattr(fd, &oldtio);
        bzero(&newtio, sizeof(newtio));

        newtio.c_cflag = B115200|CS8|CLOCAL|CREAD;

        newtio.c_iflag = IGNPAR;
        newtio.c_oflag = 0;
        newtio.c_lflag = 0;
        newtio.c_cc[VTIME] = 0;
        newtio.c_cc[VMIN] = 1;

        tcflush(fd, TCIFLUSH);
        tcsetattr(fd, TCSANOW, &newtio);

        return fd;
}

void close_serial()
{
	close(roomba_fd);
}

int check_target(void)
{
        struct termios newtio;
        int fd;
        unsigned char cmd[2] = { (unsigned char)SENSORS,
                                (unsigned char)SENSORS_ALL };
        unsigned char read_buf[32];

        fd = open(USBSERIAL, O_RDWR | O_NOCTTY | O_NONBLOCK);
        if (fd < 0){
                perror("/dev/ttyUSB) is not Exist!");
                exit(EXIT_FAILURE);
        }
        tcgetattr(fd, &oldtio);
        bzero(&newtio, sizeof(newtio));

        newtio.c_cflag = B115200|CS8|CLOCAL|CREAD;
        newtio.c_iflag = IGNPAR;
        newtio.c_oflag = 0;
        newtio.c_lflag = 0;
        newtio.c_cc[VTIME] = 20;
        newtio.c_cc[VMIN] = 0;

        tcflush(fd, TCIFLUSH);
        tcsetattr(fd, TCSANOW, &newtio);

        write(fd, cmd, 2);
        usleep(1000);
        read(fd, read_buf, 26);

        close(fd);
        return 0;
}

int send_byte(unsigned char send_data)
{
	int value;
	value = write(roomba_fd, &send_data, 1);
	return value;
}

int send_bytes(unsigned char *send_data, int size)
{
	int value;
	value = write(roomba_fd, send_data, size);
	return value;
}

void start_roomba()
{
	roomba_mode = MODE_PASSIVE;
	send_byte(START);
}

void control_roomba()
{
	roomba_mode = MODE_SAFE;
	send_byte(CONTROL);
}

void safe_roomba()
{
	roomba_mode = MODE_SAFE;
	send_byte(SAFE);
}

void full_roomba()
{
	roomba_mode = MODE_FULL;
	send_byte(FULL);
}

void poweroff_roomba()
{
	roomba_mode = MODE_UNKNOWN;
	send_byte(POWER);
}

void clean_roomba()
{
	roomba_mode = MODE_PASSIVE;
	send_byte(CLEAN);
}

void drive(int velocity, int radius ) {
        int result;
        unsigned char cmd[5] = { (unsigned char)DRIVE,
				(unsigned char)(velocity>>8),
				(unsigned char)(velocity&0xff),
				(unsigned char)(radius >> 8),
				(unsigned char)(radius & 0xff) };
        result = send_bytes (cmd, 5);
}

void stop() {
	drive(0, 0);
}

void go_forward() {
	drive(200, 0x8000);
}

void go_backward() {
	drive(-200, 0x8000);
}

void turn_left() {
	drive(200, 129);
}

void turn_right() {
	drive(200, -129);
}

int set_motion(int cmd, int value)
{
	int result = 0;

	switch(cmd){
	case FORWARD_MOTION:
		if (go == 0) {
			usleep(10000);
			go = 1;
			back = 0;
			go_forward();
			printf("GoForward motion was set\n");
		}
		else {
			go = 0;
			back = 0;
			stop();
			printf("GoForward motion was not set\n");
		}
		result = go;
		break;
	case RIGHTTURN_MOTION:
		if(dm_right == 0){
			dm_right = 1;
			dm_left = 0;
			turn_right();
			printf("TurnRight motion was set\n");
		}
		else{
			dm_right = 0;
			stop();
			printf("TurnRight motion was not set\n");
		}
		result = dm_right;
		break;
	case BACKWARD_MOTION:
		if(back == 0){
			usleep(10000);
			back = 1;
			go = 0;
			go_backward();
			printf("GoBackward motion was set\n");
		}
		else{
			back = 0;
			go = 0;
			stop();
			printf("GoBackward motion was not set\n");
		}
		result = back;
		break;
	case LEFTTURN_MOTION:
		if(dm_left == 0){
			dm_right = 0;
			dm_left = 1;
			turn_left();
			printf("TurnLeft motion was set\n");
		}
		else{
			dm_left = 0;
			stop();
			printf("TurnLeft motion was not set\n");
		}
		result = dm_left;
		break;
	case STOP_MOTION:
		back = 0;
		go = 0;
		dm_right = 0;
		dm_left = 0;
		stop();
		break;
	}
	return result;
}

void read_sensors() {
        int result;
        unsigned char cmd[2] = { (unsigned char)SENSORS,
                                (unsigned char)SENSORS_ALL };
        unsigned char buf[32];
        result = send_bytes (cmd, 2);
        usleep(1000);
        result = read(roomba_fd, buf, sizeof(buf));
        /*
        if (result == 26)
                printf("bump: %c%c wheel: %c%c%c wall: %c cliff: %c%c%c%c vwal: %d motr: %d dirtL: %d dirtR: %d remo: %x butt: %x dist: %d angl: %d chst: %d volt: %d curr: %d temp: %d chrg: %d capa: %d\n",
                        (buf[BUMPSWHEELDROPS] & BUMPLEFT_MASK)?"l":"_",
                        (buf[BUMPSWHEELDROPS] & BUMPRIGHT_MASK)?"r":"_",
                        (buf[BUMPSWHEELDROPS] & WHEELDROPLEFT_MASK)?"l":"_",
                        (buf[BUMPSWHEELDROPS] & WHEELDROPCENT_MASK)?"c":"_",
                        (buf[BUMPSWHEELDROPS] & WHEELDROPRIGHT_MASK)?"r":"_",
                        buf[WALL]?"Y":"n",
                        buf[CLIFFLEFT]?"l":"_",
                        buf[CLIFFFRONTLEFT]?"L":"_",
                        buf[CLIFFFRONTRIGHT]?"R":"_",
                        buf[CLIFFRIGHT]?"r":"_",
                        buf[VIRTUALWALL],
                        buf[MOTOROVERCURRENTS],
                        buf[DIRTLEFT] & 0xff,
                        buf[DIRTRIGHT] & 0xff,
                        buf[REMOTEOPCODE],
                        buf[BUTTONS],
                        (short)((buf[DISTANCE_HI] << 8) | (buf[DISTANCE_LO] & 0xff)),
                        (short)((buf[ANGLE_HI] << 8) | (buf[ANGLE_LO] & 0xff)),
                        buf[CHARGINGSTATE] & 0xff,
                        (int)(buf[VOLTAGE_HI] & 0xff) << 8 | buf[VOLTAGE_LO] & 0xff,
                        (short)((buf[CURRENT_HI] << 8) | (buf[CURRENT_LO] & 0xff)),
                        buf[TEMPERATURE],
                        (int)(buf[CHARGE_HI] & 0xff) << 8 | buf[CHARGE_LO] & 0xff,
                        (int)(buf[CAPACITY_HI] & 0xff) << 8 | buf[CAPACITY_LO] & 0xff);
        */
}

int get_sensor_data(int type, unsigned short *send_data)
{
	int result = 0;
	unsigned char cmd[] = {(unsigned char)SENSORS, (unsigned char)SENSORS_ALL};
	result = send_bytes(cmd, 2);

	usleep(5000);

	result = read(roomba_fd, send_data, 26); // SENSOR READ
	if (result != 26) {
		printf("Success to read sensor data!\n");
		return 1;
	} else {
		printf("Success to read sensor data!\n");
		return 0;
	}
}

int get_sensor_lightbumper(int type, unsigned short *send_data)
{
	int result = 0;
	unsigned char cmd[] = {(unsigned char)SENSORS, (unsigned char)SENSORS_LIGHTBUMPER};
	result = send_bytes(cmd, 2);

	usleep(5000);

	result = read(roomba_fd, send_data, 12); // SENSOR READ
	if (result != 12) {
		printf("Success to read sensor lightbumper data!\n");
		return 1;
	} else {
		printf("Success to read sensor lightbumper data!\n");
		return 0;
	}
}


static int sensor_thread_run = 0;
static char values[7][20];
static char *pvalues[7] = {values[0], values[1], values[2], values[3], values[4], values[5], values[6]};
void *sensor_thread(void *arg)
{
	int sleeptime = 2;
	unsigned char send_data[26];
	char value[20];

	while (sensor_thread_run) {
		sleep((unsigned int)sleeptime);

		get_sensor_data(DISTANCE_SENSOR, send_data);
		/*
		sprintf(value, "%d", send_data[1]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_FRONT_DISTANCE, value);
		sprintf(value, "%d", send_data[2]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_BACK_DISTANCE, value);
		sprintf(value, "%d", send_data[3]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_LEFT_DISTANCE, value);
		sprintf(value, "%d", send_data[4]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_RIGHT_DISTANCE, value);
		*/
		sprintf(values[0], "%d", (int)(send_data[VOLTAGE_HI] & 0xff) << 8 | send_data[VOLTAGE_LO] & 0xff);
		sprintf(values[1], "%d", (short)((send_data[CURRENT_HI] << 8) | (send_data[CURRENT_LO] & 0xff)));
		sprintf(values[2], "%d", send_data[TEMPERATURE]);
		sprintf(values[3], "%d", (int)(send_data[CHARGE_HI] & 0xff) << 8 | send_data[CHARGE_LO] & 0xff);

		/*
		sprintf(value, "%d", send_data[1]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_X, value);
		sprintf(value, "%d", send_data[2]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_Y, value);
		sprintf(value, "%d", send_data[3]);
		upnp_set_variable(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_ACCELERATOR_Z, value);
		*/
		sprintf(values[4], "%d", (short)((send_data[DISTANCE_HI] << 8) | (send_data[DISTANCE_LO] & 0xff)));
		sprintf(values[5], "%d", (short)((send_data[ANGLE_HI] << 8) | (send_data[ANGLE_LO] & 0xff)));
		sprintf(values[6], "%d", send_data[CHARGINGSTATE] & 0xff);
		//printf("values=%s\n", *values);
		upnp_set_variables(SMS_SERVICE_INDEX, SERVICE_SMS_INDEX_FRONT_DISTANCE, pvalues, 7);
		
	}

	return NULL;
}

int set_sensor_data(int type, int index, int value)
{
	int result = 0;

	if (type != LIGHT_SENSOR)
		return -1;

	switch(index){
	case FRONT_LIGHT_SENSOR:
		front = (front == 0) ? 1 : 0;
		printf("Front light sets to be %s\n", front ? "ON":"OFF");
		result = front;
		break;
	case FOG_LIGHT_SENSOR:
		fog = (fog == 0) ? 1 : 0;
		printf("Fog light sets to be %s\n", fog ? "ON":"OFF");
		result = fog;
		break;
	case BREAK_LIGHT_SENSOR:
		breakl = (breakl == 0) ? 1 : 0;
		printf("Break light sets to be %s\n", breakl ? "ON":"OFF");
		result = breakl;
		break;
	case WINKER_LIGHT_SENSOR:
		if (value == 0) {
			emer = (emer == 0) ? 1 : 0;
			printf("Emergency light sets to be %s\n", emer ? "ON":"OFF");
			result = emer;
		}
		else if (value == 1) {
			left = (left == 0) ? 1 : 0;
			printf("Leftturn light sets to be %s\n", emer ? "ON":"OFF");
			result = left;
		}
		else if (value == 2) {
			right = (right == 0) ? 1 : 0;
			printf("Rightturn light sets to be %s\n", emer ? "ON":"OFF");
			result = right;
		}
		break;
	}
	return result;
}

int get_camera_data(int id, int *type, char *encoding)
{
	if (id != camera_id)
		return -1;

	*type = camera_type;  /* 0:stream, 1:still */

	strcpy(encoding, camera_encoding); /* motion-jpeg: "mjpg", "mjpeg" */

	return 0;
}

int set_camera_data(int id, int type, char *encoding)
{
	if (id != 0)
		return -1;

	if (type == 0 || type == 1) /* 0:stream,  1:still */
		camera_type = type;

	strcpy(camera_encoding, encoding); /* motion-jpeg: "mjpg", "mjpeg" */

	return 0;
}

void init_device(int argc, char *argv[])
{
	roomba_fd = open_serial();
	//check_target();
	start_roomba();
	control_roomba();

	start_camera_thread();
}

void finish_device(int argc, char *argv[])
{
        sensor_thread_run = 0;

        global.stop = 1;
        usleep(1000 * 1000);

        close_serial();
        global.in[0].stop(0);
        global.out[0].stop(global.out[0].param.id);
        pthread_cond_destroy(&global.in[0].db_update);
        pthread_mutex_destroy(&global.in[0].db);

        return;
}

int start_sensor_thread()
{
	if (sensor_thread_run) {
		printf("Sensor thread is already running!\n");
		return -1;
	}
	printf("Thread Create\n");
	sensor_thread_run = 1;
	if(pthread_create(&sensor_thread_id, NULL, sensor_thread, NULL) < 0) {
		printf("sensor thread create error\n");
		return -1;
	}

	return 0;
}

int split_parameters(char *parameter_string, int *argc, char **argv)
{
    int count = 1;
    argv[0] = NULL; // the plugin may set it to 'INPUT_PLUGIN_NAME'
    if(parameter_string != NULL && strlen(parameter_string) != 0) {
        char *arg = NULL, *saveptr = NULL, *token = NULL;

        arg = strdup(parameter_string);

        if(strchr(arg, ' ') != NULL) {
            token = strtok_r(arg, " ", &saveptr);
            if(token != NULL) {
                argv[count] = strdup(token);
                count++;
                while((token = strtok_r(NULL, " ", &saveptr)) != NULL) {
                    argv[count] = strdup(token);
                    count++;
                    if(count >= MAX_PLUGIN_ARGUMENTS) {
                        IPRINT("ERROR: too many arguments to input plugin\n");
                        return 0;
                    }
                }
            }
        }
    }
    *argc = count;
    return 1;
}

int start_camera_thread()
{
    char *input[MAX_INPUT_PLUGINS];
    char *output[MAX_OUTPUT_PLUGINS];

    input[0] = "--resolution 640x480 --fps 5 --device /dev/video0";
    global.incnt = 1;

    output[0] = "--port 8080";
    global.outcnt = 1;

    if(pthread_mutex_init(&global.in[0].db, NULL) != 0) {
        LOG("could not initialize mutex variable\n");
        closelog();
        exit(EXIT_FAILURE);
    }
    if(pthread_cond_init(&global.in[0].db_update, NULL) != 0) {
        LOG("could not initialize condition variable\n");
        closelog();
        exit(EXIT_FAILURE);
    }

    global.in[0].param.parameters = strchr(input[0], ' ');
    split_parameters(global.in[0].param.parameters, &global.in[0].param.argc, global.in[0].param.argv);
    global.in[0].param.global = &global;
    global.in[0].param.id = 0;
    if(input_init(&global.in[0].param, 0)) {
        LOG("input_init() return value signals to exit\n");
        closelog();
        exit(0);
    }
    global.out[0].param.parameters = strchr(output[0], ' ');
    split_parameters(global.out[0].param.parameters, &global.out[0].param.argc, global.out[0].param.argv);
    global.out[0].param.global = &global;
    global.out[0].param.id = 0;
    if(output_init(&global.out[0].param, 0)) {
        LOG("output_init() return value signals to exit\n");
        closelog();
        exit(0);
    }
    DBG("starting input plugin\n");
    if(input_run(0)) {
        LOG("can not run input plugin\n");
        closelog();
        return 1;
    }
    DBG("starting output plugin\n");
    output_run(0);

    return 0;
}
